package dijkstra;

import java.util.Collections;
import java.util.HashSet;
import java.util.Set;

import be.kuleuven.cs.som.annotate.Basic;
import roborally.factory.PhysicalAmountFactory;
import roborally.factory.PositionFactory;
import roborally.itemsonboard.Robot;
import roborally.supporting.PhysicalAmount;
import roborally.supporting.DefaultConstants;
import roborally.supporting.Unit;
import roborally.supporting.Position;

/**
 * To use this class properly make an object and execute the object. Then you can ask the proper values.
 * The shortest path in this case is the path to a destination consuming the least energy. It calculates
 * all the positions the robot can reach. The complexity is high, depending the energyLimit of the robot.
 * 
 * @invar The position to reach must be valid.
 * 		| isValidPositionToReach(positionToReach,robot)
 * 
 * @author Molderez Tom, 2de bach Ingenieurswetenschappen Computerwetenschappen-Elektrotechniek and Reniers Jorn, 2de bach Ingenieurswetenschappen Werktuigkunde-Computerwetenschappen
 * @version 2.0
 *
 */
public class ReachAllPossiblePositions extends Dijkstra {

	private static final double INFINITY = Double.POSITIVE_INFINITY;
	
	/**
	 * Creates a new ReachAllPossiblePossitions object with the given parameters.
	 * 
	 * @param robot The robot of this ReachAllPossiblePossitions object.
	 * @pre The robot must be valid.
	 * 		| isValidRobot(robot)
	 * @effect 
	 * 		| super(robot)
	 */
	public ReachAllPossiblePositions(Robot robot) {
		super(robot);
	}
	
	/**
	 * Returns the visited vertexes. The minimum distance from the getRobot() to vertexes in this set are known.
	 * 
	 * @pre The Dijkstra object must be in a valid state.
	 * 		| this.getState() == DijkstraState.EXECUTED_REACH_ALL_POSITIONS
	 */
	@Basic
	public Set<Vertex> getVisitedVertexes() {
		assert this.getState() == DijkstraState.EXECUTED_REACH_ALL_POSITIONS;
		return Collections.unmodifiableSet(getVisited());
	}
	
	/**
	 * Performs the Dijkstra algorithm with some modifications. The method first performs an estimation of all the positions 
	 * the getRobot() can reach. This is an upper limit. Next the method calculates the minimal energy to reach all those positions
	 * using the Dijkstra algorithm. If the energy needed is higher then the energy level of the getRobot(), the vertex containing
	 * the particular information is removed from visited.
	 * After invoking this method, the method getVisitedVertexes() will return a set with all the possible vertexes the getRobot()
	 * can perform and this vertexes contain the minimal energy required to get there.
	 * 
	 * @post
	 * 		| for each position in {(0,0), (0,1), (1,0), ..., (this.getRobot().getBoard().getXLimit(),this.getRobot().getBoard().getYLimit()) }
	 * 		|	(new this).getVisitedVertexes().contains(position) || this.getRobot().getEnergyRequiredToReach(position)<0
	 * 		| for each Vertex in {(new this).getVisitedVertexes()}
	 * 		|	this.getRobot().getEnergyRequiredToReach(position).equals(Vertex.getEnergyToReach())
	 */
	@Override
	public void run() {
		if(getState() == DijkstraState.NOT_EXECUTED) {
			initialise();
			intialiseEstimation();
			
			do {
				Vertex lusVertex = getUnvisited().poll();
			
				assert !getVisited().contains(lusVertex);
			
				getVisited().add(lusVertex);
				neighborsEstimation(lusVertex);
				
				if(lusVertex.getEnergyToReach().compareTo(this.getRobot().getEnergyLevel()) == 1) {
					estimation.remove(lusVertex);
					getVisited().remove(lusVertex);
				}
				
			} while(!getUnvisited().isEmpty());
			setState(DijkstraState.EXECUTED_REACH_ALL_POSITIONS);
		}
	}
	
	/**
	 * Initializes the storage containers used by the method estimateAll. A upper limit of all the positions that
	 * the this.getRobot() can reach will be added to the unvisited queue.
	 */
	private void intialiseEstimation(){
		PhysicalAmount movementCost = DefaultConstants.ROBOT_ENERGY_MOVEMENT.add(PhysicalAmountFactory.get().create(this.getRobot().getWeightCarried().getAmount() * DefaultConstants.ROBOT_ENERGY_MOVEMENT_PER_WEIGHT, Unit.WATTSECOND));
		
		Long xMin = Math.max( 0, (long) (getRobot().getPosition().getX()-movementCost.getAsFractionOfThis(getRobot().getEnergyLevel())) );
		Long yMin = Math.max( 0, (long) (getRobot().getPosition().getY()-movementCost.getAsFractionOfThis(getRobot().getEnergyLevel())) );
		xMin=Math.max(0, xMin-1); // -1 to be sure that it is an upper limit of all reachable positions
		yMin=Math.max(0, yMin-1);	// -1 to be sure that it is an upper limit of all reachable positions
		
		Long xMax, yMax;
		
		Long temp= (long) (getRobot().getPosition().getX()+movementCost.getAsFractionOfThis(getRobot().getEnergyLevel())) +1;   //overflow can occur.
		if(temp > getRobot().getBoard().getXLimit()) // +1 to be sure that it is an upper limit of all reachable positions
			xMax = getRobot().getBoard().getXLimit();
		else
			xMax = temp;
			
		temp= (long) (getRobot().getPosition().getY()+movementCost.getAsFractionOfThis(getRobot().getEnergyLevel())) +1;   //overflow can occur.
		if(temp > getRobot().getBoard().getYLimit()) // +1 to be sure that it is an upper limit of all reachable positions
			yMax = getRobot().getBoard().getYLimit();
		else
			yMax = temp;
		
		for(Long ix = xMin; ix <= xMax; ix++)
			for(Long iy = yMin; iy <= yMax; iy++)
				if(getRobot().canHaveAsPosition(PositionFactory.get().create(ix,iy)) && estimationReacheble(ix,iy,movementCost))
					estimation.add(new Vertex(PositionFactory.get().create(ix,iy), null, PhysicalAmountFactory.get().create(0,Unit.WATTSECOND)));
	}
	
	/**
	 * Estimates the minimum energy to reach a particular position. Be careful, this is an under limit. The
	 * real energy will always be equal or more.
	 * 
	 * @param x The x coordinate of the target.
	 * @param y The y coordinate of the target.
	 * @param movementCost The movementCost to move one step.
	 * @return
	 * 		| result == getRobot().getPosition().getManhattanDistance(PositionFactory.getInstance().newPosition(x,y))*movementCost <= getRobot().getEnergyLevel()
	 */
	private boolean estimationReacheble(Long x, Long y, PhysicalAmount movementCost){
		PhysicalAmount temp = movementCost.multiply(getRobot().getPosition().getManhattanDistance(PositionFactory.get().create(x,y)));
		return ( temp.compareTo(getRobot().getEnergyLevel())== -1 || temp.compareTo(getRobot().getEnergyLevel())== 0 );
	}
	
	/**
	 * A set containing all vertexes of which positions that can be reached by this getRobot(), plus some extra. This is an upper limit
	 * of the positions of the vertexes that the getRobot() can reach.
	 */
	private final Set<Vertex> estimation = new HashSet<Vertex>();
	
	/**
	 * Calculates the neighbors of the given vertex. Only the vertexes that have a position on which the board can contain the getRobot() and that are reachable for the robot are processed.
	 * If the vertexes are already visited or they are not in the estimation set, nothing happens.
	 * Else calculate the energy needed to move from this vertex to the given position. If this is the 
	 * first time we reach that position a new vertex is created and stored. Otherwise the existing vertex is
	 * modified if the energy to reach that vertex is cheaper than the energy of the vertex (the previous 'minimal' energy).
	 * 
	 * @param vertex The vertex from which the neighbors must be calculated. 
	 */
	private void neighborsEstimation(Vertex vertex){
		for(Position lusPosition : vertex.getPosition().getLegalNeighbors()) {
			if(getRobot().getBoard().canHaveAsItem(new Robot(lusPosition))) {
				Vertex lusVertex = new Vertex(lusPosition, null, PhysicalAmountFactory.get().create(INFINITY,Unit.WATTSECOND));
				if(!getVisited().contains(lusVertex) && estimation.contains(new Vertex(lusPosition,null,PhysicalAmountFactory.get().create(INFINITY,Unit.WATTSECOND)))) {
					int neededTurns = turnsBeforeMovement(vertex, lusVertex);
					lusVertex.setOrientation(vertex.getOrientation().turn(neededTurns));
					PhysicalAmount energy = calculateEnergy(vertex, lusVertex, neededTurns);
					if(!getDistances().containsKey(lusVertex) || energy.compareTo(getDistances().get(lusVertex))==-1) {
						setCheapestPath(lusVertex, energy);
						getCurrentPath().put(vertex, lusVertex);
					}
				}
			}
		}
	}

}
